﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using Coding4Fun.Cannon.Core.Settings;
using Phidgets;
using Phidgets.Events;

namespace Coding4Fun.Cannon.Robot
{
	public class PhidgetServo : IDisposable
	{
		private static AdvancedServo _servoController;
		private ServoSettings _settings;
		readonly AutoResetEvent _waitForLoad = new AutoResetEvent(false); // prevent race event to be sure phidgets fully loaded and online
		private const int PanIndex = 0;
		private const int TiltIndex = 1;
		private Random _randomNumber = new Random((int)DateTime.Now.Ticks % int.MaxValue);
        private static double _currentPanPosition;
        private static double _currentTiltPosition;


		public PhidgetServo(ServoSettings settings) 
		{
			if (_servoController == null)
			{
				_settings = settings;
				_servoController = new AdvancedServo();
				_servoController.Attach += _servoController_Attach;
                _servoController.PositionChange += _servoController_PositionChange;
				//_servoController.Detach += new DetachEventHandler(advServo_Detach);
				//_servoController.Error += new ErrorEventHandler(advServo_Error);

				//_servoController.CurrentChange += new CurrentChangeEventHandler(advServo_CurrentChange);
				//_servoController.PositionChange += new PositionChangeEventHandler(advServo_PositionChange);
				//_servoController.VelocityChange += new VelocityChangeEventHandler(advServo_VelocityChange);
				_waitForLoad.Reset();

				if(!_servoController.Attached)
					_servoController.open();
			}
		}

        void _servoController_PositionChange(object sender, PositionChangeEventArgs e)
        {
            if (e.Index == PanIndex)
                _currentPanPosition = e.Position;
            else if (e.Index == TiltIndex)
                _currentTiltPosition = e.Position;
        }

		private static bool isLoaded = false;
		void _servoController_Attach(object sender, AttachEventArgs e)
		{
			isLoaded = true;
			InitalizeSettings();
			_waitForLoad.Set();
		}

		private AdvancedServoServo GetPanServo()
		{
			if (!isLoaded)
			    _waitForLoad.WaitOne();
			return _servoController.servos[PanIndex];
		}

		private AdvancedServoServo GetTiltServo()
		{
			if (!isLoaded)
			    _waitForLoad.WaitOne();

			return _servoController.servos[TiltIndex];
		}

		private void InitalizeSettings()
		{
			
			var pan = GetPanServo();
			var tilt = GetTiltServo();

            pan.Type = ServoServo.ServoType.HITEC_HS5245MG;
            tilt.Type = ServoServo.ServoType.HITEC_HS5245MG;


            PanMaxPosition = _settings.PanMaxPosition;
            PanMinPosition = _settings.PanMinPosition;
            PanAcceleration = _settings.PanAcceleration;

            PanVelocityLimit = pan.VelocityMax;

            TiltMaxPosition = _settings.TiltMaxPosition;
            TiltMinPosition = _settings.TiltMinPosition;
            TiltAcceleration = _settings.TiltAcceleration;
            TiltVelocityLimit = tilt.VelocityMax;

            pan.Engaged = true;
            tilt.Engaged = true;
		}

		

		public double PanAngle
		{
			get { return TiltRawToAngleValue(GetPanServo().Position); }
			set { SetAngle(GetPanServo(), value, true); }
		}

		public double TiltAngle
		{
			get { return TiltRawToAngleValue(GetTiltServo().Position); }
			set { SetAngle(GetTiltServo(), value, false); }
		}

		public double PanVelocityLimit
		{
			get { return GetPanServo().VelocityLimit; }
			set { GetPanServo().VelocityLimit = value; }
		}

		public double TiltVelocityLimit
		{
			get { return GetTiltServo().VelocityLimit; }
			set { GetTiltServo().VelocityLimit = value; }
		}

		public double PanAcceleration
		{
			get { return GetPanServo().Acceleration; }
			set { GetPanServo().Acceleration = value; }
		}

		public double TiltAcceleration
		{
			get { return GetTiltServo().Acceleration; }
			set { GetTiltServo().Acceleration = value; }
		}

		public double PanMinPosition
		{
			get { return PanRawToAngleValue(GetPanServo().PositionMin); }
			set { GetPanServo().PositionMin = PanAngleToRawValue(value); }
		}

		public double TiltMinPosition
		{
			get { return TiltRawToAngleValue(GetTiltServo().PositionMin); }
			set { GetTiltServo().PositionMin = TiltAngleToRawValue(value); }
		}

		public double PanMaxPosition
		{
			get { return PanRawToAngleValue(GetPanServo().PositionMax); }
			set { GetPanServo().PositionMax = PanAngleToRawValue(value); }
		}

		public double TiltMaxPosition
		{
			get { return TiltRawToAngleValue(GetTiltServo().PositionMax); }
			set { GetTiltServo().PositionMax = TiltAngleToRawValue(value); }
		}


		public void SetAngle(double panAngle, double tiltAngle)
		{
			PanAngle = panAngle;
			TiltAngle = tiltAngle;
		}

		public void SetPanAngle(double angle)
		{
			PanAngle = angle;
		}

		public void SetTiltAngle(double angle)
		{
			TiltAngle = angle;
		}

		public void SetPanAndTiltRandomly()
		{
			var pan = GetPanServo();
			var tilt = GetTiltServo();
			PanAngle = _randomNumber.Next((int)PanMinPosition, (int)PanMaxPosition);
			TiltAngle = _randomNumber.Next((int)TiltMinPosition, (int)TiltMaxPosition);
		}

		private static void SetAngle(AdvancedServoServo servo, double angle, bool isPan)
		{
			try
			{
				// since we are switching to thinking in phidget servo
				// must convert to raw value first
                if(isPan)
                    angle = PanAngleToRawValue(angle);
                else
    				angle = TiltAngleToRawValue(angle);

				if (servo.PositionMax < angle)
					angle = servo.PositionMax;
				else if (servo.PositionMin > angle)
					angle = servo.PositionMin;

				servo.Position = angle;
			}
			catch(Exception)
			{

			}
		}

       

		private static double PanAngleToRawValue(double value)
		{
			return (value / 4.5) + 57.5;
		}

		private static double PanRawToAngleValue(double value)
		{
			return (value - 57.5) * 4.5;
		}

        private static double TiltAngleToRawValue(double value)
        {
            return (value / 4.5) + 77.5;
        }

        private static double TiltRawToAngleValue(double value)
        {
            return (value - 77.5) * 4.5;
        }

		//public ReturnStatus SetVelocity(int index, int velocityLimit)
		//{
		//    try
		//    {
		//        _servoController.servos[index].VelocityLimit = velocityLimit;
		//        return new ReturnStatus { Success = true };
		//    }
		//    catch (Exception ex0)
		//    {

		//        return new ReturnStatus { Success = false, ExceptionThrown = ex0 };
		//    }
		//}

		//public ReturnStatus SetVelocity(Dictionary<int, int> values)
		//{
		//    return values.Aggregate(new ReturnStatus { Success = true }, (current, value) =>
		//    {
		//        var status = SetVelocity(value.Key, value.Value);
		//        return status.Success != true ? status : current;
		//    });
		//}

		//public ReturnStatus SetAcceleration(int index, int acceleration)
		//{
		//    try
		//    {
		//        _servoController.servos[index].Acceleration = acceleration;
		//        return new ReturnStatus { Success = true };
		//    }
		//    catch (Exception ex0)
		//    {

		//        return new ReturnStatus { Success = false, ExceptionThrown = ex0 };
		//    }
		//}

		//public ReturnStatus SetAcceleration(Dictionary<int, int> values)
		//{
		//    return values.Aggregate(new ReturnStatus { Success = true }, (current, value) =>
		//    {
		//        var status = SetAcceleration(value.Key, value.Value);
		//        return status.Success != true ? status : current;
		//    });
		//}

		//public void SetZeroAngles(Dictionary<CoreTypes.ServoIndex, int> zeroAngles)
		//{ }

		//public void SetOneHundredAndEightyAngles(Dictionary<CoreTypes.ServoIndex, int> oneHundredAndEightyAngles)
		//{ }

		#region Implementation of IDisposable

		~PhidgetServo()
		{
			Dispose();
		}

		public void Dispose()
		{
			_servoController.close();
		}

		#endregion
	}
}


